const int pos_pack[N_SERVOS] = {
  deg2ms(90), deg2ms(0), deg2ms(0),
  deg2ms(90), deg2ms(0), deg2ms(0),
  deg2ms(90), deg2ms(0), deg2ms(0),
  
  deg2ms(90), deg2ms(180), deg2ms(180),
  deg2ms(90), deg2ms(180), deg2ms(180),
  deg2ms(90), deg2ms(180), deg2ms(180),
};
const int pos_packup[N_SERVOS] = {
  deg2ms(90), deg2ms(0), deg2ms(180),
  deg2ms(90), deg2ms(0), deg2ms(180),
  deg2ms(90), deg2ms(0), deg2ms(180),
  
  deg2ms(90), deg2ms(180), deg2ms(0),
  deg2ms(90), deg2ms(180), deg2ms(0),
  deg2ms(90), deg2ms(180), deg2ms(0),
};

const int pos_calibration_med[N_SERVOS] = {
  deg2ms(90), deg2ms(90), deg2ms(90),
  deg2ms(90), deg2ms(90), deg2ms(90),
  deg2ms(90), deg2ms(90), deg2ms(90),
  
  deg2ms(90), deg2ms(90), deg2ms(90),
  deg2ms(90), deg2ms(90), deg2ms(90),
  deg2ms(90), deg2ms(90), deg2ms(90),
};

int force_positions(const int pos[])
{
  int max_mov = 0;
  
  for(int i=0; i<N_SERVOS; i++)
  {
    int mov = set_servo_position(i, pos[i]);
    if(mov>max_mov) max_mov=mov;
  }
  
  return max_mov;
}

int set_pack_position()
{
  return force_positions(pos_pack);
  
  // Only for manual calibration
  //return force_positions(pos_calibration_med);
  //return force_positions(pos_packup);
}


